#include "./motors.h"
#include "../xlib/gpio.h"
#include "./sensor.h"
#include "../rtx/rtx_os.h"

namespace pearl_driver {

Motors motors;

namespace motors_impl {

using namespace stm32f1::gpio;
using namespace stm32f1;

using DIR1 = PC<8>;
using DIR2 = PC<9>;
using DUMP = PC<11>;

enum class Dir : uint8_t {
    CW,
    CCW,
    BRAKE,
};

static void set_dir(Dir dir) {
    switch (dir) {
        case Dir::CW: 
            DIR1::set();
            DIR2::clr();
        break;

        case Dir::CCW:
            DIR1::clr();
            DIR2::set();
        break;

        case Dir::BRAKE:
            DIR1::clr();
            DIR2::clr();
        break;
    }
}

}

using namespace motors_impl;

void Motors::init() {

    rcc::enable<DIR1, DIR2, DUMP>();
    config<
        Cfg<DIR1, Mode::OUT_50MHz_PP>,
        Cfg<DIR2, Mode::OUT_50MHz_PP>,
        Cfg<DUMP, Mode::OUT_50MHz_PP>
    >();
    DIR1::clr();
    DIR2::clr();
    DUMP::clr();
}

Error Motors::dump_top() {
    Error err;
    uint32_t time_count = 0;
    uint32_t alm_count = 0;

    if (sensor.is_dump_top()) {
        goto finish;
    }

    set_dir(Dir::CW);
    DUMP::set();

    for (;;) {
        if (sensor->is_dump_top()) {
            break;
        }

        if ((time_count >= 500) && (sensor.adc_value() > DUMP_ALM)) {
            alm_count ++;
            if (alm_count >= 5) {
                err = ec::pearl::DUMP_MOTOR_ALM;
                goto finish;
            }
        } else {
            alm_count = 0;
        }

        osDelay(50);
        time_count += 50;
        if (time_count >= DUMP_TIMEOUT) {
            err = ec::pearl::DUMP_MOTOR_TIMEOUT;
            goto finish;
        }
    }

finish:
    set_dir(Dir::BRAKE);
    DUMP::clr();
    return err;
}

Error Motors::dump_bottom() {
    Error err;
    uint32_t time_count = 0;
    uint32_t alm_count = 0;
    
    if (sensor.is_dump_bottom()) {
        goto finish;
    }

    set_dir(Dir::CCW);
    DUMP::set();

    for (;;) {
        if (sensor->is_dump_bottom()) {
            break;
        }

        if ((time_count >= 500) && (sensor.adc_value() > DUMP_ALM)) {
            alm_count ++;
            if (alm_count >= 5) {
                err = ec::pearl::DUMP_MOTOR_ALM;
                goto finish;
            }
        } else {
            alm_count = 0;
        }

        osDelay(50);
        time_count += 50;
        if (time_count >= DUMP_TIMEOUT) {
            err = ec::pearl::DUMP_MOTOR_TIMEOUT;
            goto finish;
        }
    }

finish:
    set_dir(Dir::BRAKE);
    DUMP::clr();
    return err;
}

}
